# -*- coding:utf-8 -*-
# @FileName :generate_dlc_path.py
# @Time :2024/5/4 15:59
# @Author :ShengYe
# @Des :生成dlc路径
import math
import numpy as np
from carla import Transform, Location, Rotation
from matplotlib import pyplot as plt

from planner.global_planning import RoadOption


class CustomPath:
    def __init__(self, transform, road_option):
        self.transform = transform
        self.road_option = road_option


def get_waypoints_1(start_point):
    def draw_double_line_y(x):
        r1 = 0.096 * (x - 60) - 1.2
        r2 = 0.096 * (x - 120) - 1.2
        dm1 = 25
        dm2 = 25
        dn1 = -9
        dn2 = -9
        yr = dn1 / 2 * (1 + math.tanh(r1)) - dn2 / 2 * (1 + math.tanh(r2))
        return yr

    def draw_double_line_fai(x):
        r1 = 0.096 * (x - 60) - 1.2
        r2 = 0.096 * (x - 120) - 1.2
        dm1 = 25
        dm2 = 25
        dn1 = -9
        dn2 = -9
        fai = math.atan(dn1 * (1 / math.cosh(r1)) ** 2 * (1.2 / dm1) - dn2 * (1 / math.cosh(r2)) ** 2 * (1.2 / dm2))
        return fai

    waypoints = []

    for i in range(1, 180):
        location = Location(x=i + start_point.location.x, y=start_point.location.y + draw_double_line_y(i), z=0.0)
        # 创建旋转信息
        rotation = Rotation(pitch=0.0, yaw=start_point.rotation.yaw + draw_double_line_fai(i), roll=0.0)
        # 创建Transform对象
        transform = Transform(location, rotation)
        # 创建Waypoint对象
        waypoint = CustomPath(transform, RoadOption.LANE_FOLLOW)
        waypoint_tuple = (waypoint, RoadOption.LANE_FOLLOW)
        waypoints.append(waypoint_tuple)

    return waypoints


def get_waypoints_2(start_point):
    print("【进入获取路径模块】start_point", start_point)
    shape = 2.4
    dm1 = 25
    dm2 = 25
    dn1 = 3.6
    dn2 = 3.6
    Xs1 = 60
    Xs2 = 120
    N = 180  # 采样长度

    X_phi = [i for i in range(N)]
    X_ref = X_phi

    Y_ref = []
    phi_ref = []

    for p in range(N):
        r1 = shape / dm1 * (X_ref[p] - Xs1) - shape / 2
        r2 = shape / dm2 * (X_ref[p] - Xs2) - shape / 2
        Y_ref.append(dn1 / 2 * (1 + np.tanh(r1)) - dn2 / 2 * (1 + np.tanh(r2)))
        phi_ref.append(
            np.arctan(dn1 * (1 / np.cosh(r1)) ** 2 * (1.2 / dm1) - dn2 * (1 / np.cosh(r2)) ** 2 * (1.2 / dm2))
        )

    waypoints = []
    for i in range(len(X_ref)):
        location = Location(x=X_ref[i] + start_point.location.x, y=start_point.location.y - Y_ref[i], z=0.0)
        rotation = Rotation(pitch=0.0, yaw=phi_ref[i] + start_point.rotation.yaw, roll=0.0)
        transform = Transform(location, rotation)
        waypoint = CustomPath(transform, RoadOption.LANE_FOLLOW)
        waypoint_tuple = (waypoint, RoadOption.LANE_FOLLOW)
        waypoints.append(waypoint_tuple)
    print("【路径生成成功】")
    return waypoints
